#include <assert.h>
#include <xc.h>
#include <stdbool.h>
#include <stdint.h>
#include "../GenericTypeDefs.h"
#include "Uart1.h"
#include "Framework.h"
#include "CommandHandler.h"
#include "PanelComm.h"

#define U1_BUF_LEN  (256)
#define TX1_FULL  (U1STA & _U1STA_UTXBF_MASK)
#define RX1_FULL  (U1STA & _U1STA_URXDA_MASK)

RxPkt u1RxPkt;
TxPkt u1TxPkt;

uint8_t u1RxHeadIdx = 0;
uint8_t u1RxTailIdx = 0;
uint8_t u1TxHeadIdx = 0;
uint8_t u1TxTailIdx = 0;
uint8_t u1RxDataBuf[U1_BUF_LEN];
uint8_t u1TxDataBuf[U1_BUF_LEN];

static uint16_t IdxModLen(uint16_t idx);
static bool ExtendAxis_ChkAndClearPackValidFlag(void);
static void HandleExtendAxisCommand(void);
static void ExtendAxis_PutByteInTxBuf(uint8_t byte, uint16_t offset);

void U1Debug(uint8_t data) {
    uint8_t i;
    for (i = 0; i < 5; i++) {
        u1TxDataBuf[u1TxTailIdx] = data;
        u1TxTailIdx = (u1TxTailIdx + 1) & (U1_BUF_LEN - 1); 
    }
}

void Uart1Comm_Init(void) {
    u1RxHeadIdx = 0;
    u1RxTailIdx = 0;
    u1TxHeadIdx = 0;
    u1TxTailIdx = 0;
}

static uint16_t IdxModLen(uint16_t idx) {
    return idx & (U1_BUF_LEN - 1);
}

void ExtendAxisComm_RxTx(void) {
    while (RX1_FULL) {
        u1RxDataBuf[u1RxTailIdx] = U1RXREG;
        u1RxTailIdx = (u1RxTailIdx + 1) & (U1_BUF_LEN - 1);
    }

    while (!TX1_FULL) {
        if(u1TxHeadIdx != u1TxTailIdx) {
            U1TXREG = u1TxDataBuf[u1TxHeadIdx];
            u1TxHeadIdx = (u1TxHeadIdx + 1) & (U1_BUF_LEN - 1);
        } else {
            break;
        }
    }
}

static bool ExtendAxis_ChkAndClearPackValidFlag(void) {
    if (u1RxPkt.isRxFinish) {
        u1RxPkt.isRxFinish = false;
        return true;
    } else {
        return false;
    }
}

void ExtendAxisEnterBoot(void) {
    uint8_t buf[5];
    uint16_t crc;
    buf[0] = 0x61;
    buf[1] = 0x00;
    buf[2] = 0x00;
    crc = CRC16_Chk(buf, 3);
    buf[3] = crc >> 8;
    buf[4] = crc;

    uint8_t i;
    for (i = 0; i < 5; i++) {
        u1TxDataBuf[IdxModLen(u1TxTailIdx)] = buf[i];
        u1TxTailIdx = (u1TxTailIdx + 1) & (U1_BUF_LEN - 1); 
    }
}

void ExtendAxis_BuildRecPack(void) {
    while (true) {
        uint8_t rxLen = 0;
        if (u1RxTailIdx < u1RxHeadIdx) {
            rxLen = u1RxTailIdx + U1_BUF_LEN - u1RxHeadIdx;
        } else {
            rxLen = u1RxTailIdx - u1RxHeadIdx;
        }

        if(rxLen < 5) {
            return;
        }

        u1RxPkt.pktHd = u1RxDataBuf[u1RxHeadIdx];
        if(u1RxPkt.pktHd != 0xAA) {
            u1RxHeadIdx = IdxModLen(u1RxHeadIdx + 1);
            continue;
        }

        u1RxPkt.cmd	= u1RxDataBuf[IdxModLen(u1RxHeadIdx + 1)];
        if (u1RxPkt.cmd < READ_VER_INFO_BACK || u1RxPkt.cmd > CHECK_CRC_BACK) {
            u1RxHeadIdx = IdxModLen(u1RxHeadIdx + 1);
            continue;
        }
        
        u1RxPkt.dataLen	= u1RxDataBuf[IdxModLen(u1RxHeadIdx + 2)];
        u1RxPkt.pktLen = u1RxPkt.dataLen + 5;
        if(rxLen < u1RxPkt.pktLen) {
            return;
        }

        uint8_t i = 0;
        for(i = 0; i < u1RxPkt.dataLen; i++) {
            u1RxPkt.dataBuf[i] = u1RxDataBuf[IdxModLen(u1RxHeadIdx + 3 + i)];
        }

        uint8_t CRC_L, CRC_H;
        CRC_L = u1RxDataBuf[IdxModLen(u1RxHeadIdx + u1RxPkt.dataLen + 3)];
        CRC_H = u1RxDataBuf[IdxModLen(u1RxHeadIdx + u1RxPkt.dataLen + 4)];
        u1RxPkt.crcRx = (CRC_H << 8) | CRC_L;
        u1RxPkt.crcGen = Crc_Fun(&u1RxPkt.cmd, u1RxPkt.dataLen + 2, 0);

        if (u1RxPkt.crcRx != u1RxPkt.crcGen) {
            u1RxHeadIdx = IdxModLen(u1RxHeadIdx + 1);
            continue;
        }

        u1RxPkt.isRxFinish = true;
        u1RxHeadIdx = IdxModLen(u1RxHeadIdx + u1RxPkt.pktLen);

        break;
	}
}

static void HandleExtendAxisCommand(void) {
    uint8_t cmd = u1RxPkt.cmd;
     switch (cmd) { 
        case READ_VER_INFO_BACK: {
            PanelCommRplyPacket.Cmd = READ_VER_INFO_BACK;   // 0xF4
            PanelCommRplyPacket.DataByteLength = u1RxPkt.dataLen;
            uint8_t i;
            for (i = 0; i < u1RxPkt.dataLen; i++) {
                PanelCommRplyPacket.Data[i] = u1RxPkt.dataBuf[i];
            }
            break;
        }
        
        case SET_MODE_BACK: {
            return;
        }
        
        case ERASE_FLASH_BACK: {
            PanelCommRplyPacket.Cmd = ERASE_FLASH_BACK;     // 0xF6
            PanelCommRplyPacket.Data[0] = u1RxPkt.dataBuf[0];
            PanelCommRplyPacket.DataByteLength = 1;
            break;
        }
        
        case PROGRAM_FLASH_BACK: {
            PanelCommRplyPacket.Cmd = PROGRAM_FLASH_BACK;   // 0xF7
            PanelCommRplyPacket.Data[0] = u1RxPkt.dataBuf[0];
            PanelCommRplyPacket.DataByteLength = 1;
            break;
        }
        
        case CHECK_CRC_BACK: {
            PanelCommRplyPacket.Cmd = CHECK_CRC_BACK;       // 0xF8
            PanelCommRplyPacket.Data[0] = u1RxPkt.dataBuf[0];
            PanelCommRplyPacket.DataByteLength = 1;
            break;
        }
        
        default:
            return;
    }

    PopulateRplyPack();
    PanelComm_FillTxBytesWithRplyPack();
}

void ExtendAxis_CommandHandler_Execute(void) {
    if (ExtendAxis_ChkAndClearPackValidFlag()) {
        HandleExtendAxisCommand();
    }
}

void BuildTxPktToExtendAxis() {
    u1TxPkt.pktHd = 0xAA;
    u1TxPkt.crcGen = Crc_Fun(&u1TxPkt.cmd, u1TxPkt.dataLen + 2, 0);
}

static void ExtendAxis_PutByteInTxBuf(uint8_t byte, uint16_t offset) {
    u1TxDataBuf[IdxModLen(u1TxTailIdx + offset)] = byte;
}

void ExtendAxis_FillTxBytesWithRplyPack(void) {
    ExtendAxis_PutByteInTxBuf(u1TxPkt.pktHd, 0);
    ExtendAxis_PutByteInTxBuf(u1TxPkt.cmd, 1);
    ExtendAxis_PutByteInTxBuf(u1TxPkt.dataLen, 2);

    uint8_t i;
    for (i = 0; i < u1TxPkt.dataLen; i++) {
        ExtendAxis_PutByteInTxBuf(u1TxPkt.dataBuf[i], 3 + i);
    }
    
    ExtendAxis_PutByteInTxBuf(u1TxPkt.crcGen, 3 + u1TxPkt.dataLen);
    ExtendAxis_PutByteInTxBuf(u1TxPkt.crcGen >> 8, 3 + u1TxPkt.dataLen + 1);

    u1TxTailIdx = IdxModLen(u1TxTailIdx + 3 + u1TxPkt.dataLen + 2);
}